using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using System.Threading;

namespace BepuPhysics
{
    public class IslandSleeper
    {
        IdPool setIdPool;
        Bodies bodies;
        Solver solver;
        BroadPhase broadPhase;
        internal PairCache pairCache;
        ConstraintRemover constraintRemover;
        BufferPool pool;
        public int InitialIslandBodyCapacity { get; set; } = 1024;
        public int InitialIslandConstraintCapacity { get; set; } = 1024;

        /// <summary>
        /// 获取或设置应用于活动正文计数的乘数,该活动正文计数用于计算给定时间步内的休眠遍历数。
        /// </summary>
        public float TestedFractionPerFrame { get; set; } = 0.01f;
        /// <summary>
        /// 获取或设置作为给定帧中休眠的体数的活动集到目标的分数。
        /// 这只是一个目标;实际的睡眠身体数量可能或多或少。
        /// </summary>
        public float TargetSleptFraction { get; set; } = 0.005f;
        /// <summary>
        /// 获取或设置活动集到目标的分数,即在给定帧中为休眠而遍历的主体数。
        /// 这只是一个目标;实际遍历的实体数量可能或多或少。
        /// </summary>
        public float TargetTraversedFraction { get; set; } = 0.01f;

        public IslandSleeper(Bodies bodies, Solver solver, BroadPhase broadPhase, ConstraintRemover constraintRemover, BufferPool pool)
        {
            this.bodies = bodies;
            this.solver = solver;
            this.broadPhase = broadPhase;
            this.constraintRemover = constraintRemover;
            this.pool = pool;
            setIdPool = new IdPool(16, pool);
            // 我们为活动集保留索引0。
            setIdPool.Take();
            findIslandsDelegate = FindIslands;
            gatherDelegate = Gather;
            typeBatchConstraintRemovalDelegate = TypeBatchConstraintRemoval;
            executeRemovalWorkDelegate = ExecuteRemovalWork;
        }

        internal void ReturnSetId(int id)
        {
            setIdPool.Return(id, pool);
        }

        struct ConstraintBodyEnumerator : IForEach<int>
        {
            public QuickList<int> ConstraintBodyIndices;
            public BufferPool Pool;
            public int SourceIndex;
            public void LoopBody(int bodyIndex)
            {
                if (bodyIndex != SourceIndex)
                {
                    ConstraintBodyIndices.Add(bodyIndex, Pool);
                }
            }
        }



        struct ForcedSleepPredicate : IPredicate<int>
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool Matches(ref int bodyIndex)
            {
                return true;
            }
        }
        struct SleepPredicate : IPredicate<int>
        {
            public Bodies Bodies;
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool Matches(ref int bodyIndex)
            {
                return Bodies.ActiveSet.Activity[bodyIndex].SleepCandidate;
            }
        }
        struct TraversalTest<TPredicate> : IPredicate<int> where TPredicate : IPredicate<int>
        {
            public IndexSet PreviouslyTraversedBodies;
            public TPredicate Predicate;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool Matches(ref int bodyIndex)
            {
                // 请注意,我们挡路在翻新旧土地的单个线程上进行遍历。
                if (PreviouslyTraversedBodies.Contains(bodyIndex))
                    return false;
                // 请注意,添加到先前遍历的主体集是安全的。首先测试当前遍历的考量Body集合。
                // 如果命中,则不会执行谓词,但遍历不会停止。
                PreviouslyTraversedBodies.AddUnsafely(bodyIndex);
                return Predicate.Matches(ref bodyIndex);
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        bool PushBody<TTraversalPredicate>(int bodyIndex, ref IndexSet consideredBodies, ref QuickList<int> bodyIndices, ref QuickList<int> visitationStack,
            BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate<int>
        {
            if (!consideredBodies.Contains(bodyIndex))
            {
                if (!predicate.Matches(ref bodyIndex))
                {
                    return false;
                }
                // 这个身体还没有被穿越过。把它推到堆栈上。
                bodyIndices.Add(bodyIndex, pool);
                consideredBodies.AddUnsafely(bodyIndex);
                visitationStack.Add(bodyIndex, pool);
            }
            return true;
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        bool EnqueueUnvisitedNeighbors<TTraversalPredicate>(int bodyIndex,
            ref QuickList<int> bodyIndices,
            ref QuickList<ConstraintHandle> constraintHandles,
            ref IndexSet consideredBodies, ref IndexSet consideredConstraints,
            ref QuickList<int> visitationStack,
            ref ConstraintBodyEnumerator bodyEnumerator,
            BufferPool pool, ref TTraversalPredicate predicate) where TTraversalPredicate : IPredicate<int>
        {
            bodyEnumerator.SourceIndex = bodyIndex;
            ref var list = ref bodies.ActiveSet.Constraints[bodyIndex];
            for (int i = 0; i < list.Count; ++i)
            {
                ref var entry = ref list[i];
                if (!consideredConstraints.Contains(entry.ConnectingConstraintHandle.Value))
                {
                    // 尚未遍历此约束。按照每个其他连接实体的约束进行操作。
                    constraintHandles.Add(entry.ConnectingConstraintHandle, pool);
                    consideredConstraints.AddUnsafely(entry.ConnectingConstraintHandle.Value);
                    bodyEnumerator.ConstraintBodyIndices.Count = 0;
                    solver.EnumerateConnectedBodies(entry.ConnectingConstraintHandle, ref bodyEnumerator);
                    for (int j = 0; j < bodyEnumerator.ConstraintBodyIndices.Count; ++j)
                    {
                        var connectedBodyIndex = bodyEnumerator.ConstraintBodyIndices[j];
                        if (!PushBody(connectedBodyIndex, ref consideredBodies, ref bodyIndices, ref visitationStack, pool, ref predicate))
                            return false;
                    }
                }
            }
            return true;
        }

        /// <summary>
        /// 遍历活动约束图,收集与谓词匹配的主体。如果在遍历期间访问的任何主体与谓词匹配失败,则遍历将终止。
        /// </summary>
        /// <typeparam name="TTraversalPredicate">要测试每个正文索引的谓词的类型。</typeparam>
        /// <param name="pool">要从中分配临时集合的池。</param>
        /// <param name="startingActiveBodyIndex">开始遍历的活动正文的索引。</param>
        /// <param name="Predicate">用于测试每个遍历正文的谓词。如果任何正文导致谓词返回False,则遍历将停止,并且函数将返回False。</param>
        /// <param name="bodyIndices">列表,用于填充在孤岛收集期间遍历的正文索引。不包括不符合谓词的正文。</param>
        /// <param name="ConstraintHandles">要用岛收集期间遍历的约束句柄填充的列表。</param>
        /// <return>如果遍历模拟图但没有找到使谓词返回false的主体,则为True。如果任何主体未通过谓词,则为False。
        /// bodyIndices和ConstraintHandles列表将包含所有遍历的谓词传递正文和约束。</return>
        public bool CollectIsland<TTraversalPredicate>(BufferPool pool, int startingActiveBodyIndex, ref TTraversalPredicate predicate,
            ref QuickList<int> bodyIndices, ref QuickList<ConstraintHandle> constraintHandles) where TTraversalPredicate : IPredicate<int>
        {
            Debug.Assert(startingActiveBodyIndex >= 0 && startingActiveBodyIndex < bodies.ActiveSet.Count);
            // 我们将以深度优先的方式建造这个岛。这意味着我们积累的物体和约束将按深度优先顺序存储在任何不活动的岛屿中,
            // 对于缓存而言,这恰好是一个相当不错的布局。换句话说,当我们唤醒这些岛屿时,图中彼此靠近的物体将具有
            // 在记忆中彼此接近的几率就越高。直接相连的身体可能最终会彼此相邻,这意味着加载一个身体可能会让你免费获得另一个身体。
            // (假设它们共享高速缓存线)。
            // 约束的DFS顺序不如约束优化器的排序有用,但也不可怕。

            // 尽管是DFS,但不能保证访问堆栈会比最后一个岛屿本身小,我们也无法知道这个岛屿有多大
            // 除了不能大于整个活动模拟之外。
            var initialBodyCapacity = Math.Min(InitialIslandBodyCapacity, bodies.ActiveSet.Count);
            // 请注意,我们跟踪所有考虑过的实体和约束。
            // 虽然出于遍历的目的,我们只需要跟踪它们中的一个,但是跟踪两者都允许以较低的开销收集唯一主体和约束。
            // 请注意,约束句柄集被初始化为覆盖整个句柄跨度。
            // 这实际上很好-每个单独的对象只占用一位,所以131072个对象只使用16KB。
            var consideredBodies = new IndexSet(pool, bodies.ActiveSet.Count);
            var consideredConstraints = new IndexSet(pool, solver.HandlePool.HighestPossiblyClaimedId + 1);
            // 堆栈将存储正文索引。
            var visitationStack = new QuickList<int>(initialBodyCapacity, pool);

            // 通过有条件地推动初始实体开始遍历。
            if (!PushBody(startingActiveBodyIndex, ref consideredBodies, ref bodyIndices, ref visitationStack, pool, ref predicate))
            {
                consideredBodies.Dispose(pool);
                consideredConstraints.Dispose(pool);
                visitationStack.Dispose(pool);
                return false;
            }
            var enumerator = new ConstraintBodyEnumerator();
            enumerator.Pool = pool;
            enumerator.ConstraintBodyIndices = new QuickList<int>(4, pool);

            bool disqualified = false;
            while (visitationStack.TryPop(out var nextIndexToVisit))
            {
                if (!EnqueueUnvisitedNeighbors(nextIndexToVisit, ref bodyIndices, ref constraintHandles, ref consideredBodies, ref consideredConstraints, ref visitationStack,
                    ref enumerator, pool, ref predicate))
                {
                    disqualified = true;
                    break;
                }
            }
            enumerator.ConstraintBodyIndices.Dispose(pool);
            consideredBodies.Dispose(pool);
            consideredConstraints.Dispose(pool);
            visitationStack.Dispose(pool);
            return !disqualified;
        }

        int targetTraversedBodyCountPerThread;
        int targetSleptBodyCountPerThread;
        QuickList<int> traversalStartBodyIndices;
        IThreadDispatcher threadDispatcher;
        int jobIndex;


        struct WorkerTraversalResults
        {
            // 请注意,所有这些资源都是在每个工作人员池中分配的。处理它们时要小心。
            public IndexSet TraversedBodies;
            public QuickList<IslandScaffold> Islands;

            internal void Dispose(BufferPool pool)
            {
                for (int islandIndex = 0; islandIndex < Islands.Count; ++islandIndex)
                {
                    Islands[islandIndex].Dispose(pool);
                }
                Islands.Dispose(pool);
                TraversedBodies.Dispose(pool);
            }
        }

        Buffer<WorkerTraversalResults> workerTraversalResults;

        struct GatheringJob
        {
            public int TargetSetIndex;
            public QuickList<int> SourceIndices;
            public int StartIndex;
            public int EndIndex;
            /// <summary>
            /// 如果为true,则此作业与身体索引的子集相关。如果为False,则此作业与约束句柄的子集相关。
            /// </summary>
            public bool IsBodyJob;
            // TODO：可以成立工会,但这不是很重要。
            public int TargetSetIndexInReferenceList; // 只有在身体动作中才会出现。
            // 批次/类型批次索引仅在约束作业中设置。
            public int TargetBatchIndex;
            public int TargetTypeBatchIndex;
        }

        QuickList<GatheringJob> gatheringJobs;

        void FindIslands<TPredicate>(int workerIndex, BufferPool threadPool, ref TPredicate predicate) where TPredicate : IPredicate<int>
        {
            Debug.Assert(workerTraversalResults.Allocated && workerTraversalResults.Length > workerIndex);
            ref var results = ref workerTraversalResults[workerIndex];
            results.Islands = new QuickList<IslandScaffold>(64, threadPool);
            var bodyIndices = new QuickList<int>(Math.Min(InitialIslandBodyCapacity, bodies.ActiveSet.Count), threadPool);
            var constraintHandles = new QuickList<ConstraintHandle>(Math.Min(InitialIslandConstraintCapacity, solver.HandlePool.HighestPossiblyClaimedId + 1), threadPool);

            TraversalTest<TPredicate> traversalTest;
            traversalTest.Predicate = predicate;
            traversalTest.PreviouslyTraversedBodies = new IndexSet(threadPool, bodies.ActiveSet.Count);
            var traversedBodies = 0;
            var sleptBodies = 0;

            while (traversedBodies < targetTraversedBodyCountPerThread && sleptBodies < targetSleptBodyCountPerThread)
            {
                // 此线程仍有一些休眠预算,因此请尝试另一次遍历。
                var targetIndex = Interlocked.Increment(ref jobIndex);
                if (targetIndex >= traversalStartBodyIndices.Count)
                    break;
                var bodyIndex = traversalStartBodyIndices[targetIndex];
                if (CollectIsland(threadPool, bodyIndex, ref traversalTest, ref bodyIndices, ref constraintHandles))
                {
                    // 找到了一个可以睡觉的小岛！
                    sleptBodies += bodyIndices.Count;

                    // 请注意,睡眠谓词拒绝访问在此线程之前的任何遍历中访问的任何实体。
                    // 由此我们知道,任何新发现的孤岛在这个线程上都是独一无二的。很有可能不同的线程找到了相同的
                    // 小岛,但我们让它发生是为了避免大量的同步开销。
                    // 聚集阶段将对照所有以前的工人检查每个工人所在的岛。如果是复制品,就会被扔掉。
                    var island = new IslandScaffold(ref bodyIndices, ref constraintHandles, solver, threadPool);
                    results.Islands.Add(island, threadPool);
                }
                traversedBodies += bodyIndices.Count;
                bodyIndices.Count = 0;
                constraintHandles.Count = 0;
            }
            bodyIndices.Dispose(threadPool);
            constraintHandles.Dispose(threadPool);
            results.TraversedBodies = traversalTest.PreviouslyTraversedBodies;
        }
        void FindIslands(int workerIndex, BufferPool threadPool)
        {
            // 这是外部处理的,以便提早推进代码专门化。
            if (forceSleep)
            {
                var predicate = new ForcedSleepPredicate();
                FindIslands(workerIndex, threadPool, ref predicate);
            }
            else
            {
                var predicate = new SleepPredicate { Bodies = bodies };
                FindIslands(workerIndex, threadPool, ref predicate);
            }
        }
        Action<int> findIslandsDelegate;
        bool forceSleep;
        void FindIslands(int workerIndex)
        {
            // 我们将其分开的唯一原因是,如果只有一个线程,则主池更容易传入。
            FindIslands(workerIndex, threadDispatcher.GetThreadMemoryPool(workerIndex));
        }

        Action<int> gatherDelegate;
        unsafe void Gather(int workerIndex)
        {
            while (true)
            {
                var index = Interlocked.Increment(ref jobIndex);
                if (index >= gatheringJobs.Count)
                {
                    break;
                }
                ref var job = ref gatheringJobs[index];
                if (job.IsBodyJob)
                {
                    // 从活动集中加载一定范围的实体,并将它们存储到目标非活动实体集中。
                    ref var sourceSet = ref bodies.ActiveSet;
                    ref var inactiveSetReference = ref newInactiveSets[job.TargetSetIndexInReferenceList];
                    for (int targetIndex = job.StartIndex; targetIndex < job.EndIndex; ++targetIndex)
                    {
                        var sourceIndex = job.SourceIndices[targetIndex];
                        ref var targetSet = ref bodies.Sets[job.TargetSetIndex];
                        targetSet.IndexToHandle[targetIndex] = sourceSet.IndexToHandle[sourceIndex];
                        targetSet.Activity[targetIndex] = sourceSet.Activity[sourceIndex];
                        ref var sourceCollidable = ref sourceSet.Collidables[sourceIndex];
                        targetSet.Collidables[targetIndex] = sourceCollidable;
                        // 请注意,我们只是复制约束列表引用;我们不必重新分配它。
                        // 从活动集中删除对象时请牢记这一点。我们不想处理该列表,因为我们仍在使用它。
                        targetSet.Constraints[targetIndex] = sourceSet.Constraints[sourceIndex];
                        targetSet.LocalInertias[targetIndex] = sourceSet.LocalInertias[sourceIndex];
                        targetSet.Poses[targetIndex] = sourceSet.Poses[sourceIndex];
                        targetSet.Velocities[targetIndex] = sourceSet.Velocities[sourceIndex];

                        if (sourceCollidable.Shape.Exists)
                        {
                            // 收集宽相数据,以便稍后的活性集去除相可以将其粘贴到静电宽相结构中。
                            ref var broadPhaseData = ref inactiveSetReference.BroadPhaseData[targetIndex];
                            broadPhaseData.Reference = broadPhase.activeLeaves[sourceCollidable.BroadPhaseIndex];
                            broadPhase.GetActiveBoundsPointers(sourceCollidable.BroadPhaseIndex, out var minPtr, out var maxPtr);
                            broadPhaseData.Bounds.Min = *minPtr;
                            broadPhaseData.Bounds.Max = *maxPtr;
                        }
                    }
                }
                else
                {
                    // 从活动集中加载一系列约束,并将它们存储到目标非活动约束集中。
                    ref var targetTypeBatch = ref solver.Sets[job.TargetSetIndex].Batches[job.TargetBatchIndex].TypeBatches[job.TargetTypeBatchIndex];
                    // 我们可以在所有约束上共享单个虚拟派单,因为它们属于同一类型。然而,它们可能是不同批次的。
                    solver.TypeProcessors[targetTypeBatch.TypeId].GatherActiveConstraints(bodies, solver, ref Unsafe.As<QuickList<int>, QuickList<ConstraintHandle>>(ref job.SourceIndices), job.StartIndex, job.EndIndex, ref targetTypeBatch);
                    // 将这些约束排入队列,以便稍后删除。
                    Debug.Assert(job.StartIndex >= 0 && job.EndIndex <= targetTypeBatch.ConstraintCount && job.StartIndex < job.EndIndex);
                    for (int indexInTypeBatch = job.StartIndex; indexInTypeBatch < job.EndIndex; ++indexInTypeBatch)
                    {
                        constraintRemover.EnqueueRemoval(workerIndex, targetTypeBatch.IndexToHandle[indexInTypeBatch]);
                    }
                }
            }
        }

        int typeBatchConstraintRemovalJobCount;
        Action<int> typeBatchConstraintRemovalDelegate;

        void TypeBatchConstraintRemoval(int workerIndex)
        {
            while (true)
            {
                var index = Interlocked.Increment(ref jobIndex);
                if (index >= typeBatchConstraintRemovalJobCount)
                    break;
                constraintRemover.RemoveConstraintsFromTypeBatch(index);
            }
        }

        struct CachedBroadPhaseData
        {
            public CollidableReference Reference;
            public BoundingBox Bounds;
        }

        struct InactiveSetReference
        {
            public int Index;

            public Buffer<CachedBroadPhaseData> BroadPhaseData;
        }

        QuickList<InactiveSetReference> newInactiveSets;
        QuickList<RemovalJob> removalJobs;

        enum RemovalJobType
        {
            RemoveFromBatchReferencedHandles,
            NotifyNarrowPhasePairCache,
            AddCollidablesToStaticTree,
            RemoveBodiesFromActiveSet
        }

        struct RemovalJob
        {
            public RemovalJobType Type;
        }


        void ExecuteRemoval(ref RemovalJob job)
        {
            switch (job.Type)
            {
                case RemovalJobType.RemoveFromBatchReferencedHandles:
                    constraintRemover.RemoveConstraintsFromBatchReferencedHandles();
                    break;
                case RemovalJobType.RemoveBodiesFromActiveSet:
                    {
                        for (int setReferenceIndex = 0; setReferenceIndex < newInactiveSets.Count; ++setReferenceIndex)
                        {
                            var setIndex = newInactiveSets[setReferenceIndex].Index;
                            ref var inactiveBodySet = ref bodies.Sets[setIndex];
                            ref var inactiveConstraintSet = ref solver.Sets[setIndex];
                            for (int bodyIndex = 0; bodyIndex < inactiveBodySet.Count; ++bodyIndex)
                            {
                                ref var location = ref bodies.HandleToLocation[inactiveBodySet.IndexToHandle[bodyIndex].Value];
                                Debug.Assert(location.SetIndex == 0, "At this point, the sleep hasn't gone through so the set should still be 0.");
                                constraintRemover.TryRemoveAllConstraintsForBodyFromFallbackBatch(location.Index);
                                bodies.RemoveFromActiveSet(location.Index);
                                // 现在我们可以实际更新句柄->正文映射。
                                location.SetIndex = setIndex;
                                location.Index = bodyIndex;
                            }
                        }
                    }
                    break;
                case RemovalJobType.AddCollidablesToStaticTree:
                    {
                        for (int setReferenceIndex = 0; setReferenceIndex < newInactiveSets.Count; ++setReferenceIndex)
                        {
                            ref var setReference = ref newInactiveSets[setReferenceIndex];
                            ref var set = ref bodies.Sets[setReference.Index];
                            for (int bodyIndex = 0; bodyIndex < set.Count; ++bodyIndex)
                            {
                                ref var collidable = ref set.Collidables[bodyIndex];
                                if (collidable.Shape.Exists)
                                {
                                    ref var data = ref setReference.BroadPhaseData[bodyIndex];
                                    collidable.BroadPhaseIndex = broadPhase.AddStatic(data.Reference, ref data.Bounds);
                                }
                                else
                                {
                                    // 这不是严格要求-如果没有形状,则不应使用宽相位索引。但它有助于捕获无效用法。
                                    collidable.BroadPhaseIndex = -1;
                                }
                            }
                        }
                    }
                    break;
                case RemovalJobType.NotifyNarrowPhasePairCache:
                    {
                        // 这必须是本地顺序的,因为它会导致从对缓存的全局重叠映射中删除,并从主池中分配。

                        // 虽然精确计算集合构建器需要多少空间有点棘手,因为它需要检查各个条目类型,
                        // 我们可以根据身体来近似它。
                        int largestBodyCount = 0;
                        for (int i = 0; i < newInactiveSets.Count; ++i)
                        {
                            var setCount = bodies.Sets[newInactiveSets[i].Index].Count;
                            if (setCount > largestBodyCount)
                                largestBodyCount = setCount;
                        }
                        // 我们只是随意猜测每具身体有几双。这可能是错误的,但没关系-如果需要,它会调整大小。只是不想经常调整大小。
                        var setBuilder = new SleepingSetBuilder(pool, largestBodyCount * 4, largestBodyCount);
                        for (int setReferenceIndex = 0; setReferenceIndex < newInactiveSets.Count; ++setReferenceIndex)
                        {
                            pairCache.SleepTypeBatchPairs(ref setBuilder, newInactiveSets[setReferenceIndex].Index, solver);
                        }
                        setBuilder.Dispose(pool);
                    }
                    break;
            }
        }
        Action<int> executeRemovalWorkDelegate;
        void ExecuteRemovalWork(int workerIndex)
        {
            while (true)
            {
                var index = Interlocked.Increment(ref jobIndex);
                if (index >= removalJobs.Count)
                    break;
                ExecuteRemoval(ref removalJobs[index]);
            }
        }

        struct HandleComparer : IComparerRef<int>
        {
            public Buffer<BodyHandle> Handles;
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public int Compare(ref int a, ref int b)
            {
                return Handles[a].Value.CompareTo(Handles[b].Value);
            }
        }

        int scheduleOffset;

        [Conditional("DEBUG")]
        unsafe void PrintIsland(ref IslandScaffold island)
        {
            Console.Write($"{island.BodyIndices.Count} body handles: ");
            for (int i = 0; i < island.BodyIndices.Count; ++i)
                Console.Write($"{bodies.ActiveSet.IndexToHandle[island.BodyIndices[i]]}, ");
            Console.WriteLine();
            int constraintCount = 0;
            for (int batchIndex = 0; batchIndex < island.Protobatches.Count; ++batchIndex)
            {
                ref var batch = ref island.Protobatches[batchIndex];
                for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                {
                    ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
                    constraintCount += typeBatch.Handles.Count;
                }
            }
            Console.Write($"{constraintCount} constraint handles: ");
            ReferenceCollector bodyIndexEnumerator;
            var constraintReferencedBodyHandles = new QuickSet<int, PrimitiveComparer<int>>(8, pool);
            for (int batchIndex = 0; batchIndex < island.Protobatches.Count; ++batchIndex)
            {
                ref var batch = ref island.Protobatches[batchIndex];
                for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                {
                    ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
                    var typeProcessor = solver.TypeProcessors[typeBatch.TypeId];
                    var references = stackalloc int[typeProcessor.BodiesPerConstraint];
                    bodyIndexEnumerator.References = references;
                    for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.Handles.Count; ++indexInTypeBatch)
                    {
                        var handle = typeBatch.Handles[indexInTypeBatch];
                        ref var location = ref solver.HandleToConstraint[handle];
                        Debug.Assert(location.SetIndex == 0);
                        Debug.Assert(location.TypeId == typeBatch.TypeId);
                        ref var solverBatch = ref solver.Sets[0].Batches[location.BatchIndex];
                        bodyIndexEnumerator.Index = 0;
                        typeProcessor.EnumerateConnectedBodyIndices(
                            ref solverBatch.TypeBatches[solverBatch.TypeIndexToTypeBatchIndex[location.TypeId]], location.IndexInTypeBatch, ref bodyIndexEnumerator);
                        for (int i = 0; i < typeProcessor.BodiesPerConstraint; ++i)
                        {
                            constraintReferencedBodyHandles.AddRef(ref bodies.ActiveSet.IndexToHandle[references[i]].Value, pool);
                        }
                        Console.Write($"{handle}, ");
                    }
                }
            }
            Console.WriteLine();
            Console.Write("Unique body handles referenced by all constraints: ");
            for (int i = 0; i < constraintReferencedBodyHandles.Count; ++i)
            {
                Console.Write($"{constraintReferencedBodyHandles[i]}, ");
            }
            Console.WriteLine();
        }


        unsafe void Sleep(ref QuickList<int> traversalStartBodyIndices, IThreadDispatcher threadDispatcher, bool deterministic, int targetSleptBodyCount, int targetTraversedBodyCount, bool forceSleep)
        {
            // 有四个线程阶段可供休眠：
            // 1)遍历约束图以识别满足休眠条件的"模拟岛"。
            // 2)收集支持模拟岛的主体和约束的数据,并将其放入非活动存储表示(BodySet和ConstraintSet)中。
            // 3)移除实体,一些与移除的约束相关的求解器记账,以及宽泛的阶段工作。
            // 4)从它们的类型批中移除休眠约束。
            // 将其分成这些阶段可以实现相当直接的并行化。
            // 遍历并行进行,咬紧牙关,因为不同线程上的不同遍历起点有时可能标识同一个岛。
            // 一旦检测到所有孤岛,第二阶段就能够消除重复项并并行收集剩余的唯一孤岛。
            // 最后,虽然删除涉及许多顺序操作,但是存在一些完全并行的部分,并且一些局部顺序部分可以彼此并行运行。

            // 这里的目标不一定是加快最好的情况-使用四个调度基本上保证20us到50us的开销-
            // 而是试图防止最坏的情况丢帧,并提高睡眠响应性。

            if (bodies.ActiveSet.Count == 0 || traversalStartBodyIndices.Count == 0)
                return;

            int threadCount = threadDispatcher == null ? 1 : threadDispatcher.ThreadCount;
            // 由于不可预测的终止条件,多线程孤岛搜索目前是不确定的。
            var workerTraversalThreadCount = deterministic ? 1 : threadCount;

            targetSleptBodyCountPerThread = Math.Max(1, targetSleptBodyCount / workerTraversalThreadCount);
            targetTraversedBodyCountPerThread = Math.Max(1, targetTraversedBodyCount / workerTraversalThreadCount);

            // 1)遍历
            this.traversalStartBodyIndices = traversalStartBodyIndices;

            pool.Take(workerTraversalThreadCount, out workerTraversalResults);
            // 请注意,工作器结果集中的所有资源都在工作器池中分配,因为线程可能需要调整对象的大小。
            this.threadDispatcher = threadDispatcher;
            jobIndex = -1;
            this.forceSleep = forceSleep;
            if (workerTraversalThreadCount > 1)
            {
                threadDispatcher.DispatchWorkers(findIslandsDelegate);
            }
            else
            {
                FindIslands(0, pool);
            }
            this.threadDispatcher = null;


            // 在没有岛屿可供睡眠的情况下,应提早出动,以避免以后的调度。
            int totalIslandCount = 0;
            for (int i = 0; i < workerTraversalThreadCount; ++i)
            {
                totalIslandCount += workerTraversalResults[i].Islands.Count;
            }
            void DisposeWorkerTraversalResults()
            {
                if (workerTraversalThreadCount > 1)
                {
                    // 遍历辅助资源的源是每线程池。
                    for (int workerIndex = 0; workerIndex < workerTraversalThreadCount; ++workerIndex)
                    {
                        workerTraversalResults[workerIndex].Dispose(threadDispatcher.GetThreadMemoryPool(workerIndex));
                    }
                }
                else
                {
                    // 遍历辅助资源的源是主池,因为它在单线程模式下运行。
                    workerTraversalResults[0].Dispose(pool);
                }
                pool.Return(ref workerTraversalResults);
            }
            if (totalIslandCount == 0)
            {
                DisposeWorkerTraversalResults();
                return;
            }

            // 2)集合
            // 现在已完成遍历。我们应该在workerTraversalResults中为每个工人提供一组结果。现在是收集睡眠身体和约束条件的所有数据的时候了。
            // 请注意,我们只预分配固定大小。这通常会被高估,但这没问题。调整大小更令人担忧。
            // (我们可以预先计算出确切的工作数量,但这并不是真正必要的。)
            var objectsPerGatherJob = 64;
            gatheringJobs = new QuickList<GatheringJob>(512, pool);
            // 我们现在只为这组独特的岛屿创造就业机会。虽然每个工作者都避免在本地创建重复项,但线程不会通信,因此可能找到了相同的孤岛。
            // 每个工人输出一组他们遍历的实体。使用它,我们可以通过检查所有以前的工人来有效地检查给定的岛是否重复。
            // 如果前一名工人穿越了一具身体,则持有该身体的后一名工人所在的岛被认为是重复的。
            // (请注意,一个躯干只能属于一个孤岛。如果两个线程找到相同的主体,这意味着它们找到了完全相同的岛,只是使用了不同的路径。它们完全是多余的。)
            newInactiveSets = new QuickList<InactiveSetReference>(32, pool);
            var sleptBodyCount = 0;
            for (int workerIndex = 0; workerIndex < workerTraversalThreadCount; ++workerIndex)
            {
                ref var workerIslands = ref workerTraversalResults[workerIndex].Islands;
                for (int j = 0; j < workerIslands.Count; ++j)
                {
                    ref var island = ref workerIslands[j];
                    bool skip = false;
                    for (int previousWorkerIndex = 0; previousWorkerIndex < workerIndex; ++previousWorkerIndex)
                    {
                        Debug.Assert(island.BodyIndices.Count > 0, "Any reported island should have a positive number of bodies in it. Otherwise, there's nothing to sleep!");
                        if (workerTraversalResults[previousWorkerIndex].TraversedBodies.Contains(island.BodyIndices[0]))
                        {
                            // 之前的一名工人已经报告了这个岛。这是复制品,跳过它。
                            skip = true;
                            break;
                        }
                    }
                    if (!skip)
                    {
                        // 为新岛分配空间。
                        var setIndex = setIdPool.Take();
                        newInactiveSets.EnsureCapacity(newInactiveSets.Count + 1, pool);
                        var referenceIndex = newInactiveSets.Count;
                        ref var newSetReference = ref newInactiveSets.AllocateUnsafely();
                        newSetReference.Index = setIndex;
                        // 我们在这里分配了宽泛的阶段数据缓冲区,但是数据是在工作线程上收集的。
                        pool.Take(island.BodyIndices.Count, out newSetReference.BroadPhaseData);
                        EnsureSetsCapacity(setIndex + 1);
                        bodies.Sets[setIndex] = new BodySet(island.BodyIndices.Count, pool);
                        bodies.Sets[setIndex].Count = island.BodyIndices.Count;
                        sleptBodyCount += island.BodyIndices.Count;
                        ref var constraintSet = ref solver.Sets[setIndex];
                        if (island.Protobatches.Count > 0)
                        {
                            constraintSet = new ConstraintSet(pool, island.Protobatches.Count);
                            for (int batchIndex = 0; batchIndex < island.Protobatches.Count; ++batchIndex)
                            {
                                ref var sourceBatch = ref island.Protobatches[batchIndex];
                                ref var targetBatch = ref constraintSet.Batches.AllocateUnsafely();
                                targetBatch = new ConstraintBatch(pool, sourceBatch.TypeIdToIndex.Length);
                                for (int typeBatchIndex = 0; typeBatchIndex < sourceBatch.TypeBatches.Count; ++typeBatchIndex)
                                {
                                    ref var sourceTypeBatch = ref sourceBatch.TypeBatches[typeBatchIndex];
                                    ref var targetTypeBatch = ref *targetBatch.CreateNewTypeBatch(sourceTypeBatch.TypeId, solver.TypeProcessors[sourceTypeBatch.TypeId], sourceTypeBatch.Handles.Count, pool);
                                    targetTypeBatch.ConstraintCount = sourceTypeBatch.Handles.Count;
                                }
                            }
                        }

                        // 单个岛屿可能涉及多个工作,具体取决于其大小。
                        // 为简单起见,作业只能覆盖连续区域。换言之,如果有两个类型批次,则至少会有两个作业-即使类型批次
                        // 每个只有一个约束。
                        // 作业也只涵盖实体或约束,不能同时涵盖两者。
                        // TODO：此作业调度模式经常出现。如果能把它统一起来就好了。使用泛型滥用的明显的零开销方法。
                        {
                            var jobCount = Math.Max(1, island.BodyIndices.Count / objectsPerGatherJob);
                            var bodiesPerJob = island.BodyIndices.Count / jobCount;
                            var remainder = island.BodyIndices.Count - bodiesPerJob * jobCount;
                            var previousEnd = 0;
                            gatheringJobs.EnsureCapacity(gatheringJobs.Count + jobCount, pool);
                            for (int i = 0; i < jobCount; ++i)
                            {
                                var bodiesInJob = i < remainder ? bodiesPerJob + 1 : bodiesPerJob;
                                ref var job = ref gatheringJobs.AllocateUnsafely();
                                job.IsBodyJob = true;
                                job.SourceIndices = island.BodyIndices;
                                job.StartIndex = previousEnd;
                                previousEnd += bodiesInJob;
                                job.EndIndex = previousEnd;
                                job.TargetSetIndex = setIndex;
                                job.TargetSetIndexInReferenceList = referenceIndex;
                            }
                        }

                        for (int batchIndex = 0; batchIndex < island.Protobatches.Count; ++batchIndex)
                        {
                            ref var sourceBatch = ref island.Protobatches[batchIndex];
                            for (int typeBatchIndex = 0; typeBatchIndex < sourceBatch.TypeBatches.Count; ++typeBatchIndex)
                            {
                                ref var sourceTypeBatch = ref sourceBatch.TypeBatches[typeBatchIndex];
                                var jobCount = Math.Max(1, sourceTypeBatch.Handles.Count / objectsPerGatherJob);
                                var constraintsPerJob = sourceTypeBatch.Handles.Count / jobCount;
                                var remainder = sourceTypeBatch.Handles.Count - constraintsPerJob * jobCount;
                                var previousEnd = 0;
                                gatheringJobs.EnsureCapacity(gatheringJobs.Count + jobCount, pool);
                                for (int i = 0; i < jobCount; ++i)
                                {
                                    var constraintsInJob = i < remainder ? constraintsPerJob + 1 : constraintsPerJob;
                                    ref var job = ref gatheringJobs.AllocateUnsafely();
                                    job.IsBodyJob = false;
                                    job.SourceIndices = sourceTypeBatch.Handles;
                                    job.StartIndex = previousEnd;
                                    previousEnd += constraintsInJob;
                                    job.EndIndex = previousEnd;
                                    job.TargetSetIndex = setIndex;
                                    job.TargetBatchIndex = batchIndex;
                                    job.TargetTypeBatchIndex = typeBatchIndex;
                                }
                            }
                        }

                        if (island.Protobatches.Count > solver.FallbackBatchThreshold)
                        {
                            // 将备用批次数据拉入新的备用批次。请注意,这不仅仅是一个浅层复制;我们正在将所有分配推送到主池中。
                            // 它们以前位于特定于线程的池中。(目前在技术上并不需要这样做,但是如果我们更改每个线程池,那么这样做既便宜又方便
                            // 利用他们的拨款通常是短暂的性质。)
                            FallbackBatch.CreateFrom(ref island.FallbackBatch, pool, out constraintSet.Fallback);
                        }
                    }
                }
            }

            constraintRemover.Prepare(threadDispatcher);

            jobIndex = -1;
            if (threadCount > 1)
            {
                threadDispatcher.DispatchWorkers(gatherDelegate);
            }
            else
            {
                Gather(0);
            }
            DisposeWorkerTraversalResults();
            gatheringJobs.Dispose(pool);

            // 3)簿记清盘
            // 阶段4仅从类型批处理中移除约束。仍然需要更新活动集约束批次、主体等。
            // 注意,当我们使用与狭窄阶段相同的ConstraintRemover时,我们不需要执行每个主体的约束列表删除或处理返回。

            // 我们不希望静电树在删除期间调整大小。这将使用主池,并与NotifyNarrowPhasePairCache作业对主池的使用发生冲突。
            broadPhase.EnsureCapacity(broadPhase.ActiveTree.LeafCount, broadPhase.StaticTree.LeafCount + sleptBodyCount);
            // 请注意,我们在第三阶段执行之前创建刷新作业。这将在将被引用的约束移除器中创建一些必要的内部结构。
            typeBatchConstraintRemovalJobCount = constraintRemover.CreateFlushJobs(deterministic);

            removalJobs = new QuickList<RemovalJob>(4, pool);
            // 较重的本地顺序作业被提前调度,留下较小的后期任务来填补空白。
            removalJobs.AllocateUnsafely() = new RemovalJob { Type = RemovalJobType.NotifyNarrowPhasePairCache };
            removalJobs.AllocateUnsafely() = new RemovalJob { Type = RemovalJobType.RemoveBodiesFromActiveSet };
            removalJobs.AllocateUnsafely() = new RemovalJob { Type = RemovalJobType.AddCollidablesToStaticTree };
            removalJobs.AllocateUnsafely() = new RemovalJob { Type = RemovalJobType.RemoveFromBatchReferencedHandles };

            jobIndex = -1;
            if (threadCount > 1)
            {
                threadDispatcher.DispatchWorkers(executeRemovalWorkDelegate);
            }
            else
            {
                for (int i = 0; i < removalJobs.Count; ++i)
                {
                    ExecuteRemoval(ref removalJobs[i]);
                }
            }
            removalJobs.Dispose(pool);

            // 4)从类型批次中删除约束
            // 从类型批次中删除会阻止主体从激活集中删除,并且约束句柄映射更新：
            // -移除实体会导致移动的实体,与移动的实体相关联的任何约束都必须更新其实体索引以指向新位置。
            // 从类型批处理中并行移除约束会导致争用条件。
            // 详细说明：类型批量删除使用约束句柄映射来定位约束。更新句柄映射会引入争用条件。
            // 由于大岛(或多个岛)的类型批量移除将倾向于创建足够的工作来使用CPU,
            // 我们暂缓所有其他工作(除了约束句柄映射更新之外,目前类型批处理约束删除依赖于约束句柄)
            // 进入第三阶段。它采用一系列本地顺序作业的形式,因此不会有很好的负载平衡。
            // 但是完美的缩放不一定是必需的--我们只是想尽可能地减少帧丢失。
            jobIndex = -1;
            if (threadCount > 1)
            {
                threadDispatcher.DispatchWorkers(typeBatchConstraintRemovalDelegate);
            }
            else
            {
                for (int i = 0; i < typeBatchConstraintRemovalJobCount; ++i)
                {
                    constraintRemover.RemoveConstraintsFromTypeBatch(i);
                }
            }

            // 在类型批处理约束删除之后存在的这个阶段是约束删除器如何工作的副产品-
            // 由于移除类型批处理中的约束可能会导致其他约束移动,因此它将改为缓存句柄并在移动过程中查找当前位置。
            // 它可以改为在移除之前排序的约束索引上工作,以保证所有索引保持有效。
            // 如果这样做了(并且也不需要其他约束句柄查找),句柄映射更新可以与类型批处理约束删除一起拉入
            // 作为多线程任务。
            // 如果您这样做了,可能会在聚集中的入队之后立即强制更新。(和往常一样,仔细检查一下,这是一件棘手的事情。)
            // 目前,我们只是按顺序进行,因为它简单且相对便宜(我希望尽快完成这项工作)。

            // 这也可以是内部多线程的。它没有以这种方式实现的唯一原因是因为它应该非常便宜。
            // 如果测量结果不是这样的话可以稍后调换。
            for (int i = 0; i < newInactiveSets.Count; ++i)
            {
                var setIndex = newInactiveSets[i].Index;
                ref var set = ref solver.Sets[setIndex];
                for (int batchIndex = 0; batchIndex < set.Batches.Count; ++batchIndex)
                {
                    ref var batch = ref set.Batches[batchIndex];
                    for (int typeBatchIndex = 0; typeBatchIndex < batch.TypeBatches.Count; ++typeBatchIndex)
                    {
                        ref var typeBatch = ref batch.TypeBatches[typeBatchIndex];
                        for (int indexInTypeBatch = 0; indexInTypeBatch < typeBatch.ConstraintCount; ++indexInTypeBatch)
                        {
                            var handle = typeBatch.IndexToHandle[indexInTypeBatch];
                            ref var constraintLocation = ref solver.HandleToConstraint[handle.Value];
                            constraintLocation.SetIndex = setIndex;
                            constraintLocation.BatchIndex = batchIndex;
                            constraintLocation.IndexInTypeBatch = indexInTypeBatch;
                            Debug.Assert(constraintLocation.TypeId == typeBatch.TypeId, "Sleeping a constraint shouldn't change its type!");
                        }
                    }
                }
            }

            for (int i = 0; i < newInactiveSets.Count; ++i)
            {
                pool.Return(ref newInactiveSets[i].BroadPhaseData);
            }
            newInactiveSets.Dispose(pool);
            constraintRemover.Postflush();
        }

        // TODO：请注意,它使用的是正文索引,而不是句柄。这与"觉醒者"有不一致之处。
        // 考虑到我们没有任何静电类型检查,赋予句柄一个实际的类型而不是一切都依赖于整数是有道理的。
        // TODO：也许明智的做法是转移到Spans或类似的地方,这样我们就不必在公共接口中依赖我们自己的迟钝类型。
        /// <summary>
        /// 强制休眠实体列表以及通过遍历这些实体的约束图可以到达的所有实体。
        /// </summary>
        /// <param name="bodyIndices">要睡眠的正文索引列表。</param>
        /// <param name="threadDispatcher">用于休眠尝试的线程调度程序(如果有)。如果为NULL,则对调用线程执行休眠。</param>
        /// <param name="defiristic">如果休眠应以更高的成本产生确定性结果,则为True,否则为False。</param>
        public void Sleep(ref QuickList<int> bodyIndices, IThreadDispatcher threadDispatcher = null, bool deterministic = false)
        {
            Sleep(ref bodyIndices, threadDispatcher, deterministic, int.MaxValue, int.MaxValue, true);
        }

        /// <summary>
        /// 强制实体和通过遍历该实体的约束图可以找到的所有实体进入睡眠状态。
        /// </summary>
        /// <param name="bodyIndex">要在活动集中睡眠的主体的索引。</param>
        public void Sleep(int bodyIndex)
        {
            // 在这里堆叠一个跨度会好得多。
            var list = new QuickList<int>(1, pool);
            list.AllocateUnsafely() = bodyIndex;
            Sleep(ref list, null);
            list.Dispose(pool);
        }

        internal void Update(IThreadDispatcher threadDispatcher, bool deterministic)
        {
            if (bodies.ActiveSet.Count == 0)
                return;

            int candidateCount = (int)Math.Max(1, bodies.ActiveSet.Count * TestedFractionPerFrame);

            var traversalStartBodyIndices = new QuickList<int>(candidateCount, pool);

            // 在活动集中均匀分布目标。每一帧都会将目标向上推一个槽。
            int spacing = bodies.ActiveSet.Count / candidateCount;

            // 日程表偏移将逐渐消失在日落中,也有可能改变活动集的大小(例如,通过睡眠)
            // 将偏移量放得太远,以至于活动集计数的单次减法是不够的。所以取而代之的是我们把它包装成零。
            if (scheduleOffset > bodies.ActiveSet.Count)
            {
                scheduleOffset = 0;
            }

            var index = scheduleOffset;
            for (int i = 0; i < candidateCount; ++i)
            {
                if (index >= bodies.ActiveSet.Count)
                {
                    index -= bodies.ActiveSet.Count;
                }
                traversalStartBodyIndices.AllocateUnsafely() = index;
                index += spacing;
            }
            ++scheduleOffset;


            if (deterministic)
            {
                // 休眠发生的顺序会影响模拟的结果。为了确保决定论,我们需要将睡眠顺序与某些东西相关联
                // 这是确定性的。我们将使用与每个活动实体相关联的句柄作为订单提供者。
                pool.Take<int>(bodies.ActiveSet.Count, out var sortedIndices);
                for (int i = 0; i < bodies.ActiveSet.Count; ++i)
                {
                    sortedIndices[i] = i;
                }
                // 句柄保证是唯一的;不需要三向分区。
                HandleComparer comparer;
                comparer.Handles = bodies.ActiveSet.IndexToHandle;
                // 待办事项：这种排序可能最终会相当昂贵。在非常大的模拟上,它甚至可能相当于模拟时间的5%。
                // 如果能在这里想出一个更好的解决方案,那就太好了。一些选项包括其他确定性来源、隐藏排序,以及可能直接在句柄上枚举。
                QuickSort.Sort(ref sortedIndices[0], 0, bodies.ActiveSet.Count - 1, ref comparer);

                // 既然我们有了一组排序的索引,我们就消除了与内存布局相关的不确定性。可以将初始目标正文索引重新映射到排序列表上。
                for (int i = 0; i < traversalStartBodyIndices.Count; ++i)
                {
                    traversalStartBodyIndices[i] = sortedIndices[traversalStartBodyIndices[i]];
                    Debug.Assert(traversalStartBodyIndices[i] >= 0 && traversalStartBodyIndices[i] < bodies.ActiveSet.Count);
                }
                pool.Return(ref sortedIndices);
            }
            Sleep(ref traversalStartBodyIndices, threadDispatcher, deterministic, (int)Math.Ceiling(bodies.ActiveSet.Count * TargetSleptFraction), (int)Math.Ceiling(bodies.ActiveSet.Count * TargetTraversedFraction), false);

            traversalStartBodyIndices.Dispose(pool);
        }

        /// <summary>
        /// 确保Body、Solver和NarrowProgress至少可以包含给定数量的集(BodySets用于Body集合,ConstraintSets用于解算器,PairSubcache用于NarrowPhase.PairCache)。
        /// </summary>
        /// <param name="setsCapacity">要保证空间的集数。</param>
        public void EnsureSetsCapacity(int setsCapacity)
        {
            var potentiallyAllocatedCount = Math.Min(setIdPool.HighestPossiblyClaimedId + 1, Math.Min(bodies.Sets.Length, Math.Min(solver.Sets.Length, pairCache.SleepingSets.Length)));
            if (setsCapacity > bodies.Sets.Length)
            {
                bodies.ResizeSetsCapacity(setsCapacity, potentiallyAllocatedCount);
            }
            if (setsCapacity > solver.Sets.Length)
            {
                solver.ResizeSetsCapacity(setsCapacity, potentiallyAllocatedCount);
            }
            if (setsCapacity > pairCache.SleepingSets.Length)
            {
                pairCache.ResizeSetsCapacity(setsCapacity, potentiallyAllocatedCount);
            }
        }

        /// <summary>
        /// 确保实体和解算器可以容纳给定数量的集。
        /// 如果现有分配的容量小于请求的SET容量,则会扩大分配。
        /// 如果现有分配大于现有的潜在分配集范围和请求集容量,则分配将缩小。
        /// 缩小不会导致现有的集合丢失。
        /// </summary>
        /// <param name="setsCapacity">要为其分配空间的集的目标数量。</param>
        public void ResizeSetsCapacity(int setsCapacity)
        {
            var potentiallyAllocatedCount = Math.Min(setIdPool.HighestPossiblyClaimedId + 1, Math.Min(bodies.Sets.Length, Math.Min(solver.Sets.Length, pairCache.SleepingSets.Length)));
            setsCapacity = Math.Max(potentiallyAllocatedCount, setsCapacity);
            bodies.ResizeSetsCapacity(setsCapacity, potentiallyAllocatedCount);
            solver.ResizeSetsCapacity(setsCapacity, potentiallyAllocatedCount);
            pairCache.ResizeSetsCapacity(setsCapacity, potentiallyAllocatedCount);
        }

        public void Clear()
        {
            setIdPool.Clear();
            // 插槽0保留给活动集。
            setIdPool.Take();
        }

        public void Dispose()
        {
            setIdPool.Dispose(pool);
        }



    }
}
